#include <Servo.h>
#include <math.h>
#define PI 3.14159265
Servo S1;
Servo S2;
int S1max=180,S1min=23;//maksymalny i minimalny kąt 1 serva
int S2max=180,S2min=0;//maksymalny i minimalny kąt 2 serva 
int a=167,b=145,c=53,e=85;//wymiary w mm

void check(float,float,float);//funkcja liczy kąty jakie potrzebujem dla serva1 i serva2


void setup() {
  Serial.begin(9600);
  pinMode(A0,INPUT_PULLUP);
  S1.attach(9);//tuuuuuu
  S2.attach(10);//tuuuuuu
  S2.write(90);
  S1.write(90);
  delay(10);
}
 
void loop() {
  if(digitalRead(A0)==0)
  {
    int i = S2.read();
    int j = S1.read();

    float katServ[2];
    check(159.97,2.04,katServ);
    if(i<katServ[1]){
     for(i;i<=katServ[1];i++)
        {
        S2.write(i);
        delay(50);
        } //koniec fora 
        }
    if(i>katServ[1]){
     for(i;i>=katServ[1];i--)
        {
        S2.write(i);
        delay(50);
        } //koniec fora 
        }
    if(j<katServ[0]){
    for(j;j<=katServ[0];j++)
        {
        S1.write(j);
        delay(50);
        } //koniec fora
        }
    if(j>katServ[0]){
    for(j;j>=katServ[0];j--)
        {
        S1.write(j);
        delay(50);
        } //koniec fora
        }
   
  }//przycisk
 }


void check(float odlx,float wysy,float katServ[])
{
    int i,j;
    float h,X,D,y1,y2,kat1,kat2,odl,roznica,alfa,beta;
    roznica=100;
    

    for (i=S1min;i<=S1max;i++)
    {
        for (j=S2min; j<=S2max;j++)
        {   
            alfa=abs(i-90);
            if(i>=90)
            {   if((i+alfa)>180)
                beta=abs((j-90+alfa));
                else
                beta=j-180+alfa;
                
            }
            else
            {   
                if(j<=alfa)
                beta=abs(alfa-j);
                else
                beta=abs((j-90-alfa));
                
            }
          
       
              //Serial.print("\talfa = ");Serial.print(alfa);
              //Serial.print("\tbeta = ");Serial.print(beta);
			kat1= alfa/180*PI;//kąt w radianach
            kat2= beta/180*PI;
         	 //Serial.print("\tarad = ");Serial.print(kat1);
             //Serial.print("\tbrad = ");Serial.print(kat2);
          
            if(((i>=90) && ((j+alfa)>=180)) || ((i<90) && (j<=alfa)))
              {
                D=cos(kat2)*c;
                y2=sin(kat2)*c;
                X=cos(kat1)*b;
                y1=sin(kat1)*b;
              }
            else
              {
                X=cos(kat1)*b;
                D=sin(kat2)*c;
                y1=sin(kat1)*b;
                y2=cos(kat2)*c;
              }

            if((i>=90) && ((j+alfa)>=180))
            h=a+y1+y2-e;
            else if((i>=90) && ((j+alfa)<180))
            h=a+y1-y2-e;
            else if((i<90) && (j<=alfa))
            h=a-y1+y2-e;
            else if((i<90) && (j>alfa))
            h=a-y1-y2-e;

            if(((j>=(90-alfa)) && (i>=90)) || ((i<90) &&  (j>=(90+alfa))))
            odl=X+D;
            else
            odl=X-D;

            
            if ((roznica>=(abs(odlx-odl)+abs(wysy-h))) && (wysy<=h))
            { Serial.print(".  if.  ");
                roznica=(abs(odlx-odl)+abs(wysy-h));//wazne
                katServ[0]=i; Serial.print("S1 == "); Serial.print(i);
                katServ[1]=j; Serial.print(" S2 == "); Serial.print(j);
               
              Serial.print("\troz = "); Serial.print(roznica); 
              Serial.print("\talfa = ");Serial.print(alfa);
              Serial.print("\tbeta = ");Serial.print(beta);
              Serial.print("\todl = ");Serial.print(odl);
              Serial.print("\th = ");Serial.print(h);
             // Serial.print("\tarad = ");Serial.print(kat1);
             // Serial.print("\tbrad = ");Serial.print(kat2);
              
              Serial.print("\n");
            }

        }
    }

}//koniec check